#ifndef _ASTRA_CAMERA_H_
#define _ASTRA_CAMERA_H_

#include <deque>
#include <memory>
#include <iostream>
#include "OpenNI.h"
#include "V4l2Capture.h"
#include "logger.h"

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

class AstraCamera
{
private:
    const int32_t READ_WAIT_TIMEOUT = 200; //200ms
    enum FrameType
    {
        DEPTH = 0,
        COLOR = 1,
        IR = 2,
    };

    int32_t m_cols_;
    int32_t m_rows_;
    int32_t m_fps_;

    int32_t m_div_freq_{1};
    int32_t m_div_cnt_{0};

    bool m_camera_ready_{false};

    openni::Device m_device_;

    openni::VideoStream m_depth_stream_;
    openni::VideoStream *m_pStream_[1];
    std::shared_ptr<V4l2Capture> videoCapture;

    std::deque<std::shared_ptr<cv::Mat>> m_color_queue_;
    std::deque<std::shared_ptr<cv::Mat>> m_depth_queue_;
    std::deque<std::shared_ptr<openni::VideoFrameRef>> m_depth_frame_;

    bool initializeCameraDepth();
    bool initializeCameraColor();
    bool initializeCamera();
    bool setVideoMode(const openni::SensorInfo *sinfo, openni::VideoStream &stream, openni::PixelFormat pf);

    cv::Mat BGRToRGB(cv::Mat img);
    int convert_yuv_to_rgb_buffer(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height);
    int convert_yuv_to_rgb_pixel(int y, int u, int v);

public:
    AstraCamera(int32_t rows, int32_t cols, int32_t fps, int32_t div = 1);
    ~AstraCamera();

    bool getData(cv::Mat &color_data, cv::Mat &depth_data);
    bool getCameraFov(float &horizontalFov, float &verticalFov);
    void setConfig(const std::string message);
};

#endif /* _ASTRA_CAMERA_H_ */
